/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2018-11-27     balanceTWK   first version
 */

#include "fly.h"
#include "scope_protocol.h"

struct balance_info info = {
    .request_unlock = 0,
    .is_unlock = 0,
    .power = 0,
    .roll_p = -4.0f,
    .roll_i = -0.01f,
    .pitch_p = -4.0f,
    .pitch_i = -0.01f,
    .roll_p_in = 1.993f,
    .roll_i_in = 0.01f,
    .roll_d_in = 13.0f,
    .pitch_p_in = 1.8f,
    .pitch_i_in = 0.01f,
    .pitch_d_in = 13.0f,
};

struct balance_control control = {0};

/* defined the LED0 pin: PC0 */
#define LED_PIN1 GET_PIN(A, 8)
#define LED_PIN2 GET_PIN(A, 15)
#define KEY_PIN1 GET_PIN(A, 11)
#define KEY_PIN2 GET_PIN(A, 12)
/* ADC设备 */
#define ADC_ACCUARY ((3.3f / 4096.0f) / (10.0f / (10.0f + 40.2f)))
rt_adc_device_t adc_dev; // ADC设备句柄
int value;               // ADC0采样值
uint16_t voltage_buffer_ptr = 0;
uint16_t voltage_buffer[8];
uint16_t voltage_cal[8];

/**
 * 循环写入数据
 */
static void write_voltage_data(uint16_t value)
{
    if (voltage_buffer_ptr >= 8)
        voltage_buffer_ptr = 0;
    voltage_buffer[voltage_buffer_ptr++] = value;
}

static void bubble_sort_better(uint16_t a[], uint8_t n) // n为数组a的元素个数
{
    //最多进行N-1轮比较
    for (int i = 0; i < n - 1; i++)
    {
        uint8_t isSorted = 1;
        //每一轮比较前n-1-i个，即已排序好的最后i个不用比较
        for (int j = 0; j < n - 1 - i; j++)
        {
            if (a[j] > a[j + 1])
            {
                isSorted = 0;
                int temp = a[j];
                a[j] = a[j + 1];
                a[j + 1] = temp;
            }
        }
        if (isSorted)
            break; //如果没有发生交换，说明数组已经排序好了
    }
}

static float MedianAverageFilter(uint16_t value_buf[], uint8_t n)
{
    uint16_t sum = 0;
    bubble_sort_better(value_buf, n);
    for (uint8_t i = 1; i < n - 1; i++)
        sum += value_buf[i];
    return (float)(sum / (n - 2));
}

static void led_timer_callback()
{
    static rt_uint8_t temp = 0;
    if (temp)
    {
        rt_pin_write(LED_PIN1, PIN_HIGH);
        temp = 0;
    }
    else
    {
        rt_pin_write(LED_PIN1, PIN_LOW);
        temp++;
    }
}

int main(void)
{
    rt_err_t ret = RT_EOK;
    static rt_timer_t led_timer;
    /* LED引脚初始化 */
    rt_pin_mode(LED_PIN1, PIN_MODE_OUTPUT);
    rt_pin_mode(LED_PIN2, PIN_MODE_OUTPUT);
    led_timer = rt_timer_create("led_timer", led_timer_callback,
                                RT_NULL, 500,
                                RT_TIMER_FLAG_PERIODIC);
    rt_timer_start(led_timer);

    rt_pin_mode(KEY_PIN1, PIN_MODE_INPUT);
    rt_pin_mode(KEY_PIN2, PIN_MODE_INPUT);

#ifdef BSP_USING_ADC1
    adc_dev = (rt_adc_device_t)rt_device_find("adc1");
#endif

#ifdef BSP_USING_SPI_LCD
    lcd_show_string(10, 10, 16, "hello");
#endif

    while (1)
    {
#ifdef BSP_USING_ADC1
        rt_adc_enable(adc_dev, ADC_CHANNEL_0);
        value = rt_adc_read(adc_dev, ADC_CHANNEL_0);
        //中值滤波
        write_voltage_data(value);
        rt_memcpy(voltage_cal, voltage_buffer, 16);
        info.power = MedianAverageFilter(voltage_cal, 8) * ADC_ACCUARY;
        // protocol_report_data(PROTOCOL_CHANNEL_4, info.power);
        // rt_adc_disable(adc_dev, ADC_CHANNEL_0);
#endif
        if (info.request_unlock)
        {
            motor_unlock();
            info.request_unlock = 0;
        }
        rt_thread_mdelay(50);
    }
}
